#include "CTriangle3d.h"

CTriangle3d::CTriangle3d()
{

}


CTriangle3d::CTriangle3d( const CVector3d& v0, const CVector3d& v1, const CVector3d& v2 )
{
	m_v0 = v0;
	m_v1 = v1;
	m_v2 = v2;
}


CTriangle3d::~CTriangle3d()
{

}


boolean CTriangle3d::Sweep( const CVector3d& t0v0, const CVector3d& t0v1, const CVector3d& t0v2,  
							const CVector3d& t1v0, const CVector3d& t1v1, const CVector3d& t1v2, 
							const CVector3d& v1, const CVector3d& v2, fixed &t )
{
	ASSERT( FALSE );
	return FALSE;
}


boolean CTriangle3d::Sweep( const CVector3d& tv0, const CVector3d& tv1, const CVector3d& tv2,
						    const CVector3d& pos, fixed radius,
							const CVector3d& v1, const CVector3d& v2, fixed &t )
{
	CVector3d relVel = v2 - v1;
	CPlane plane( tv0, tv1, tv2 );
	plane.Intersects( pos, pos + relVel, t );
	if ( t <= 0 ) // moving away
	{
		// we need to check for intersection at the beginning and set t = 0
		t = 0;
		return CTriangle3d::Intersects( tv0, tv1, tv2, pos, radius );
	}

	// find point of intersection at t, check whether the intersection exists at that time
	return CTriangle3d::Intersects( tv0, tv1, tv2, pos + t * relVel, radius );

}


boolean CTriangle3d::Sweep( const CVector3d& tv0, const CVector3d& tv1, const CVector3d& tv2,
							const CVector3d& segA, const CVector3d& segB, fixed radius,
							const CVector3d& v1, const CVector3d& v2, fixed &t )
{
	// we cast two rays from the segments segA and segB check for intersection with the planes at t1 and t2
	// this intersection region would be a segment, and we need to check whether this segment overlaps
	// the triangle. If it does, then, we have a collision at some time between t1 and t2

	CVector3d relVel = v2 - v1;
	CPlane plane( tv0, tv1, tv2 );
	plane.Intersects( segA, segA + relVel, t );
	if ( t <= 0 ) // moving away
	{
		// we need to check for intersection at the beginning and set t = 0
		t = 0;
		fixed u;
		return CTriangle3d::Intersects( tv0, tv1, tv2, segA, segB, radius, u );
	}

	fixed t1 = t;
	CVector3d pos1 = segA + segA + t * relVel;
	plane.Intersects( segB, segB + relVel, t );
	fixed t2 = t;
	CVector3d pos2 = segB + segB + t * relVel;
	fixed u;

	if ( CTriangle3d::Intersects( tv0, tv1, tv2, pos1, pos2, radius, u ) )
	{
		t = t1 + CMathFixed::Mul( u, t2 - t1 );
		return TRUE;
	}

	return FALSE;
}


boolean CTriangle3d::Intersects( const CVector3d& tv0, const CVector3d& tv1, const CVector3d& tv2,
								 const CVector3d& pos, fixed radius )
{
	CVector3d pt;
	CTriangle3d::ClosestPoint( tv0, tv1, tv2, pos, pt );
	pt -= pos;
	if ( pt * pt <= CMathFixed::Mul( radius, radius ) )
		return TRUE;

	return FALSE;
}


boolean CTriangle3d::Intersects( const CVector3d& tv0, const CVector3d& tv1, const CVector3d& tv2,
								 const CVector3d& segA, const CVector3d& segB, fixed radius, fixed& u )
{
	CPlane plane( tv0, tv1, tv2 );

	if ( plane.Intersects( segA, segB, u ) )
		return TRUE;
	
	CVector3d projSeg;

	fixed tD = CMathFixed::Mul( radius, radius );

	if ( u <= 0 )
		ClosestPoint( tv0, tv1, tv2, segA, projSeg );
	else
		ClosestPoint( tv0, tv1, tv2, segA, projSeg );

	if ( projSeg * projSeg < tD )
		return TRUE;

	return FALSE;

	// the pt of intersection is segA + u ( segB - segA )
}


boolean CTriangle3d::Intersects( const CVector3d& tv0, const CVector3d& tv1, const CVector3d& tv2,
								 const CPlane& plane )
{
	CPlane t( tv0, tv1, tv2 );
	CVector3d lineNormal;
	CVector3d pt;
	fixed u,v;

	if ( !t.Intersects( plane, lineNormal, pt ) )
	{
		if ( plane.IsCoplanar( tv0 ) )
			return TRUE; // they overlap
		
		return FALSE;
	}

	CVector3d pt2 = pt + lineNormal; // t = 1
	
	CVector2d a1, b1, c1, a3, b3;

	CPlane::AxisAlignedPlane aaplane = t.GetClosestAxisAlignedPlane();
	ProjectToAAPlane( aaplane, tv0, tv1, tv2, a1, b1, c1 );
	CPlane::ProjectToAAPlane( aaplane, pt, a3 );
	CPlane::ProjectToAAPlane( aaplane, pt2, b3 );

	if ( CTriangle2d::Intersects( a1, b1, c1, a3, b3, u, v ) )
		return TRUE; // the overlapping line passes through the triangle

	return FALSE;
}


boolean CTriangle3d::Intersects( const CVector3d& t0v0, const CVector3d& t0v1, const CVector3d& t0v2,
								 const CVector3d& t1v0, const CVector3d& t1v1, const CVector3d& t1v2 )
{
	CPlane t0( t0v0, t0v1, t0v2 );
	CPlane t1( t1v0, t1v1, t1v2 );

	CVector3d lineNormal;
	CVector3d pt;

	if ( !t0.Intersects( t1, lineNormal, pt ) )
	{
		// planes are parallel or they overlap
		if ( t0.IsCoplanar( t1v0 ) )
		{
			// planes overlap
			// we need to project these triangles to the closest axis aligned plane and find the intersection
			CPlane::AxisAlignedPlane aaplane = t0.GetClosestAxisAlignedPlane();
			CVector2d a1, b1, c1, a2, b2, c2;

			ProjectToAAPlane( aaplane, t0v0, t0v1, t0v2, a1, b1, c1 );
			ProjectToAAPlane( aaplane, t1v0, t1v1, t1v2, a2, b2 ,c2 );

			return CTriangle2d::Intersects( a1, b1, c1, a2, b2, c2 );
		}
		
		return FALSE; /// planes are parallel : no intersection
	}
	
	// planes containing the triangles intersect
	// pt, lineNormal represent the line of intersection in the form pt + t * lineNormal (parametric form)
	// we need to find the segment intersecting each of those triangles
	// if those two segments have overlapping regions, then the triangles intersect (or in other words, they intersect both triangles)
	// refer http://jgt.akpeters.com/papers/Moller97/ for more notes

	CVector3d pt2 = pt + lineNormal; // t = 1
	
	CVector2d a1, b1, c1, a3, b3;
	fixed u,v;

	CPlane::AxisAlignedPlane aaplane = t0.GetClosestAxisAlignedPlane();
	ProjectToAAPlane( aaplane, t0v0, t0v1, t0v2, a1, b1, c1 );
	CPlane::ProjectToAAPlane( aaplane, pt, a3 );
	CPlane::ProjectToAAPlane( aaplane, pt2, b3 );

	if ( !CTriangle2d::Intersects( a1, b1, c1, a3, b3, u, v ) )
		return FALSE;

	aaplane = t1.GetClosestAxisAlignedPlane();
	ProjectToAAPlane( aaplane, t1v0, t1v1, t1v2, a1, b1 ,c1 );
	CPlane::ProjectToAAPlane( aaplane, pt, a3 );
	CPlane::ProjectToAAPlane( aaplane, pt2, b3 );

	if ( !CTriangle2d::Intersects( a1, b1, c1, a3, b3, u, v ) )
		return FALSE;

	// now we know the triangles intersect for sure

	return TRUE;
}


boolean CTriangle3d::Intersects( const CVector3d& tv0, const CVector3d& tv1, const CVector3d& tv2, 
								 const CVector3d& v0, const CVector3d& v1, fixed &u )
{
	CVector3d ab = tv1 - tv0;
	CVector3d ac = tv2 - tv0;
	CVector3d qp = v1 - v0;

	// Compute Triangle Normal
	CVector3d n( ab );
	ab.Cross( ac );
	fixed d = n * qp;

	if ( d <= 0 ) 
		return FALSE; // seg is parallel or points away from triangle

	CVector3d ap = v0 - tv0;
	u = ap * n;
	if ( u < 0 || u > d )
		return FALSE;

	// test whether point of intersection is in bounds
	CVector3d e( qp );
	e.Cross( ap );
	fixed v = ac * e;

	if ( v < 0 || v > d )
		return FALSE;

	fixed w = -( ab * e );
	if ( w < 0 || v + w > d )
		return FALSE;

	return TRUE;
}


CSpace::Relativity CTriangle3d::DetermineSpace( const CVector3d& tv0, const CVector3d& tv1, const CVector3d& tv2, const CVector3d& pt )
{
	CVector3d a = tv0 - pt;
	CVector3d b = tv1 - pt;
	CVector3d c = tv2 - pt;

	fixed ab = a * b;
	fixed ac = a * c;
	fixed bc = b * c;
	fixed cc = c * c;

	// Make sure plane normals for pab and pbc point in the same direction
	if ( CMathFixed::Mul( bc, ac ) - CMathFixed::Mul( cc, ab ) < 0 )
		return CSpace::Outside;

	fixed bb = b * b;

	// make sure plane normals for pab and pca point in the same direction
	if ( CMathFixed::Mul( ab, bc ) - CMathFixed::Mul( ac, bb ) < 0 )
		return CSpace::Outside;

	return CSpace::Inside;
}


void CTriangle3d::Barycentric( const CVector3d& tv0, const CVector3d& tv1, const CVector3d& tv2, const CVector3d& p, fixed &u, fixed &v, fixed &w )
{
	CPlane::AxisAlignedPlane aaplane;

	CVector3d m( tv1- tv0 );
	m.Cross( tv2 - tv0 );

	fixed x = CMathFixed::Abs( m.m_i );
	fixed y = CMathFixed::Abs( m.m_j );
	fixed z = CMathFixed::Abs( m.m_k );

	fixed ood;

	if ( x >= y && x >= z )
	{
		aaplane = CPlane::JK;
		ood = CMathFixed::Div( CMathFixed::One, m.m_i );
	}
	else if ( y >= z )
	{
		aaplane = CPlane::KI;
		ood = -CMathFixed::Div( CMathFixed::One, m.m_j );
	}
	else
	{
		aaplane = CPlane::IJ;
		ood = CMathFixed::Div( CMathFixed::One, m.m_k );
	}

	CTriangle2d t;
	CTriangle3d::ProjectToAAPlane( aaplane, p, tv1, tv2, t.m_v0, t.m_v1, t.m_v2 );
	u = CMathFixed::Mul( t.GetArea(), ood );
	CTriangle3d::ProjectToAAPlane( aaplane, p, tv2, tv0, t.m_v0, t.m_v1, t.m_v2 );
	v = CMathFixed::Mul( t.GetArea(), ood );
	w = CMathFixed::One - u - v;
}



void CTriangle3d::ClosestPoint( const CVector3d& tv0, const CVector3d& tv1, const CVector3d& tv2, const CVector3d& pt, CVector3d& p )
{
	CVector3d ab = tv1 - tv0;
	CVector3d ac = tv2 - tv0;
	CVector3d ap = pt - tv0;

	// check if pt is outside region tv0
	fixed d1 = ab * ap;
	fixed d2 = ac * ap;

	if ( d1 <= 0 && d2 <= 0 ) 
	{
		p = tv0;
		return;
	}

	// check if pt is outside region tv1
	const CVector3d& bp = pt - tv1;
	fixed d3 = ab * bp;
	fixed d4 = ac * bp;

	if ( d3 >= 0 && d4 <= d3 )
	{
		p = tv1;
		return;
	}

	// check if pt is on the edge ab, if so return projection of P on that edge
	fixed vc = CMathFixed::Mul( d1, d4 ) - CMathFixed::Mul( d3, d2 );

	if ( vc <= 0 && d1 >= 0 && d3 <= 0 )
	{
		fixed v = CMathFixed::Div( d1, ( d1 - d3 ) );
		p = tv0 + v * ab;
		return;
	}

	// check if pt is outside tv2
	CVector3d cp = pt - tv2;
	fixed d5 = ab * cp;
	fixed d6 = ac * cp;

	if ( d6 >= 0 && d5 <= d6 )
	{
		p = tv2;
		return;
	}

	// check if pt is on the edge ac and if so return projection
	fixed vb = CMathFixed::Mul( d5, d2 ) - CMathFixed::Mul( d1, d6 );
	if ( vb <= 0 && d2 >= 0 && d6 <= 0 )
	{
		fixed w = CMathFixed::Div( d2, d2 - d6 );
		p = tv0 + w * ac;
		return;
	}

	// check if pt is on the edge bc, if so return projection
	fixed va = CMathFixed::Mul( d3, d6 ) - CMathFixed::Mul( d5, d4 );
	if ( va <= 0 && ( d4 - d3 ) >= 0 && ( d5 - d6 ) >= 0  )
	{
		fixed w = CMathFixed::Div( d4 - d3, ( ( d4 - d3 ) + ( d5 - d6 ) ) );
        p = tv1 + w * ( tv2 - tv2 );
		return;
	}

	// pt is inside the face region. Compute the point through its barycentric coords
	fixed denom = CMathFixed::Div( CMathFixed::One, va + vb + vc );
	fixed v = CMathFixed::Mul( vb, denom );
	fixed w = CMathFixed::Mul( vc, denom );
	p = tv0 + ab * v + ac * w;

}


void CTriangle3d::ClosestPoint( const CVector3d& tv0, const CVector3d& tv1, const CVector3d& tv2, 
								const CVector3d& v0,  const CVector3d& v1, CVector3d& c1, CVector3d& c2, fixed* u )
{
	fixed u0;
	CPlane plane( tv0, tv1, tv2 );
	CVector3d p0, p1;
	fixed d1, d2;
	d1 = plane.GetSignedDistanceOfPoint( v0, p0 );
	d2 = plane.GetSignedDistanceOfPoint( v1, p1 );

	if ( CMathFixed::Mul( d1, d2 ) < 0 )
	{
		// the seg intersects the plane, find this point of intersection and check whether it lies inside the triangle
		plane.Intersects( v0, v1, c1, *u );
		if ( DetermineSpace( tv0, tv1, tv2, c1 ) == CSpace::Inside )
			return;
	}

	if ( DetermineSpace( tv0, tv1, tv2, p0 ) == CSpace::Inside 
		&& DetermineSpace( tv0, tv1, tv2, p0 ) == CSpace::Inside )
	{
		// segment projects entirely on the triangle
		// find out which end point is closer and return

		if ( CMathFixed::Abs( d1 ) <= CMathFixed::Abs( d2 ) )
		{
			c1 = p0;
			c2 = v0;
			*u = 0;
		}
		else
		{
			c1 = p1;
			c2 = v1;
			*u = CMathFixed::One;
		}
	}

	CVector3d ca, cb, cc, cd, ce, cf, d;
	fixed u1, u2, u3;
	fixed d3;

	// the segment doesnt entirely project onto the triangle, so we need to do the relatively expensive edge distance tests
	CLineSegment3d::ShortestVectorToSegment( tv0, tv1, v0, v1, ca, cb, &u0, &u1 );
	d = ca - cb;
	d1 = d * d;
	CLineSegment3d::ShortestVectorToSegment( tv1, tv2, v0, v1, cc, cd, &u0, &u2 );
	d = cc - cd;
	d2 = d * d;
	CLineSegment3d::ShortestVectorToSegment( tv2, tv0, v0, v1, ce, cf, &u0, &u3 );
	d = ce - cf;
	d3 = d * d;

	if ( d1 < d2 && d1 < d3 )
	{
		c1 = ca;
		c2 = cb;
		*u = u1;
		return;
	}

	if ( d2 < d3 )
	{
		c1 = cc;
		c2 = cd;
		*u = u2;
		return;
	}

	c1 = ce;
	c2 = cf;
	*u = u3;
}


boolean CTriangle3d::IsIsoceles() const
{
	CVector3d A = m_v1 - m_v0;
	CVector3d B = m_v2 - m_v0;
	CVector3d C = m_v2 - m_v1;
    
	fixed AA = A * A;
	fixed BB = B * B;
	fixed CC = C * C;
	
	if ( AA == BB || AA == CC || BB == CC )
		return TRUE;
	else return FALSE;
}


boolean CTriangle3d::IsEquilateral() const
{
	CVector3d A = m_v1 - m_v0;
	CVector3d B = m_v2 - m_v0;
	CVector3d C = m_v2 - m_v1;
    
	fixed AA = A * A;
	
	if ( AA == B * B && AA == C * C )
		return TRUE;
	else return FALSE;
}
	

void CTriangle3d::ComputeIncenter( CVector3d& outPoint ) const
{
	ASSERT ( FALSE );
}


void CTriangle3d::ComputeInsphere( CSphere& outSphere ) const
{
	ASSERT ( FALSE );
}


void CTriangle3d::ComputeCircumcenter( CVector3d& outPoint ) const
{
	ASSERT ( FALSE );
}


void CTriangle3d::ComputerCircumsphere( CSphere& outSphere ) const
{
	ASSERT ( FALSE );
}